#include "calibration.h"
#include <float.h>
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

cubic_point_t points[CALIBRATION_MAX_POINTS] = {0};
cubic_param_t *param = NULL;

void CubicFit(cubic_param_t *param, const cubic_point_t *points, int n);

void CalibrationReset(void)
{
    memset(points, 0, sizeof(points));
    memset(param, 0, sizeof(*param));
}

float CalculateForwardPower(float x)
{
    float y = param->a * x * x * x + param->b * x * x + param->c * x + param->d;
    if (y > param->max)
        return param->max; // 保证输出不大于最大值
    if (y < 0)
        return 0; // 保证输出不小于0
    return y;
}

float CalculateSWR(float forward, float reverse)
{
    if (forward <= 0.0f)
        return -1; // SWR无穷大
    if (reverse < 0.0f)
        reverse = 0.0f;
    if (fabsf(forward - reverse) < 1e-2)
        return 1.0f; // 完美匹配
    float gamma = fabsf(reverse / forward);
    if (gamma >= 1.0f)
        return -1; // SWR无穷大
    return (1.0f + gamma) / (1.0f - gamma);
}

float CalculateVoltageToWatt(float voltage)
{
#define COMPENSATION 0
#define INTERCEPT 0.02f
#define SLOPE 1.0f

    if (voltage < INTERCEPT)
        return 0.0f;

    float dBm = (voltage - INTERCEPT) / SLOPE;
    float watt = powf(10.0f, (dBm + COMPENSATION - 30.0f) / 10.0f);
    return watt;
}

void CalibrationStartFit(void)
{
    CubicFit(param, points, CALIBRATION_MAX_POINTS);
}

void CalibrationSetParam(cubic_param_t *new_param)
{
    if (new_param != NULL)
        param = new_param;
}

cubic_param_t *CalibrationGetParam(void)
{
    return param;
}

uint8_t CalibrationPointRecord(uint8_t index, float x, float y)
{
    if (index < CALIBRATION_MAX_POINTS)
    {
        points[index].x = x;
        points[index].y = y;
        return 0;
    }
    return 1;
}

// 高斯消元法求解线性方程组，带部分主元选择
static int GaussianElimination(float A[4][5], float x[4])
{
    int i, j, k;
    int max_row;
    float temp, max_val;

    // 前向消元
    for (i = 0; i < 4; i++)
    {
        // 部分主元选择：找到第i列中从第i行到第3行的最大元素
        max_row = i;
        max_val = fabsf(A[i][i]);
        for (j = i + 1; j < 4; j++)
        {
            if (fabsf(A[j][i]) > max_val)
            {
                max_val = fabsf(A[j][i]);
                max_row = j;
            }
        }

        // 如果最大主元接近零，矩阵奇异
        if (max_val < FLT_EPSILON)
        {
            return -1; // 矩阵奇异
        }

        // 交换行
        if (max_row != i)
        {
            for (j = 0; j < 5; j++)
            {
                temp = A[i][j];
                A[i][j] = A[max_row][j];
                A[max_row][j] = temp;
            }
        }

        // 归一化主元行
        float pivot = A[i][i];
        for (j = i; j < 5; j++)
        {
            A[i][j] /= pivot;
        }

        // 消元
        for (k = i + 1; k < 4; k++)
        {
            float factor = A[k][i];
            for (j = i; j < 5; j++)
            {
                A[k][j] -= factor * A[i][j];
            }
        }
    }

    // 回代
    for (i = 3; i >= 0; i--)
    {
        x[i] = A[i][4];
        for (j = i + 1; j < 4; j++)
        {
            x[i] -= A[i][j] * x[j];
        }
    }

    return 0; // 成功
}

void CubicFit(cubic_param_t *param, const cubic_point_t *points, int n)
{
    int valid_n = 0;
    float Sx = 0, Sx2 = 0, Sx3 = 0, Sx4 = 0, Sx5 = 0, Sx6 = 0;
    float Sy = 0, Sxy = 0, Sx2y = 0, Sx3y = 0;

    // 验证输入参数
    if (param == NULL || points == NULL || n <= 0)
    {
        param->a = param->b = param->c = param->d = 0;
        return;
    }

    for (int i = 0; i < n; ++i)
    {
        float x = points[i].x;
        float y = points[i].y;

        // 跳过无效数据点（NaN或无穷大）
        if (!isfinite(x) || !isfinite(y))
            continue;

        // 使用递推关系计算幂次，减少乘法操作
        float x2 = x * x;
        float x3 = x2 * x;
        float x4 = x3 * x;
        float x5 = x4 * x;
        float x6 = x5 * x;

        Sx += x;
        Sx2 += x2;
        Sx3 += x3;
        Sx4 += x4;
        Sx5 += x5;
        Sx6 += x6;
        Sy += y;
        Sxy += x * y;
        Sx2y += x2 * y;
        Sx3y += x3 * y;
        valid_n++;
    }

    // 需要至少4个有效点才能进行三次拟合
    if (valid_n < 4)
    {
        param->a = param->b = param->c = param->d = 0;
        return;
    }

    // 构造正规方程的增广矩阵
    float A[4][5] = {{(float)valid_n, Sx, Sx2, Sx3, Sy},
                     {Sx, Sx2, Sx3, Sx4, Sxy},
                     {Sx2, Sx3, Sx4, Sx5, Sx2y},
                     {Sx3, Sx4, Sx5, Sx6, Sx3y}};

    float coefficients[4];
    int result = GaussianElimination(A, coefficients);

    if (result == 0)
    {
        // 成功求解，赋值系数
        param->d = coefficients[0];
        param->c = coefficients[1];
        param->b = coefficients[2];
        param->a = coefficients[3];
    }
    else
    {
        // 矩阵奇异，返回零系数
        param->a = param->b = param->c = param->d = 0;
    }
}
